#include <dalelib/maths/v4.h>
#include <math.h>

namespace dalelib
{

#define V4_EPSILON 0.0001f

/*** constructors ***/
v4::v4() { f[0] = 0.f; f[1] = 0.f; f[2] = 0.f; f[3] = 0.f; }
v4::v4(const float _Value) { f[0] = _Value; f[1] = _Value; f[2] = _Value; f[3] = _Value; }
v4::v4(const float _Value0, const float _Value1, const float _Value2, const float _Value3) { f[0] = _Value0; f[1] = _Value1; f[2] = _Value2; f[3] = _Value3; }
const v4 v4::zero(0,0,0,0);
const v4 v4::one(1,1,1,1);

/*** accessors ***/
const v4 v4::operator =(const v4& _Value) { f[0] = _Value[0]; f[1] = _Value[1]; f[2] = _Value[2]; f[3] = _Value[3]; return *this; }

/*** unary operation ***/
v4 v4::operator-() const { return v4(-f[0],-f[1],-f[2],-f[3]); }

/*** scalar operations ***/
void v4::operator+=(const float _Value) { f[0] += _Value; f[1] += _Value; f[2] += _Value; f[3] += _Value; }
void v4::operator-=(const float _Value) { f[0] -= _Value; f[1] -= _Value; f[2] -= _Value; f[3] -= _Value; }
void v4::operator*=(const float _Value) { f[0] *= _Value; f[1] *= _Value; f[2] *= _Value; f[3] *= _Value; }
void v4::operator/=(const float _Value) { f[0] /= _Value; f[1] /= _Value; f[2] /= _Value; f[3] /= _Value; }

v4 v4::operator+(const float _Value) const { return v4( f[0]+_Value, f[1]+_Value, f[2]+_Value, f[3]+_Value ); }
v4 v4::operator-(const float _Value) const { return v4( f[0]-_Value, f[1]-_Value, f[2]-_Value, f[3]-_Value ); }
v4 v4::operator*(const float _Value) const { return v4( f[0]*_Value, f[1]*_Value, f[2]*_Value, f[3]*_Value ); }
v4 v4::operator/(const float _Value) const { return v4( f[0]/_Value, f[1]/_Value, f[2]/_Value, f[3]/_Value ); }

/*** vector operations ***/
void v4::operator+=(const v4& _Value) { f[0] += _Value[0]; f[1] += _Value[1]; f[2] += _Value[2]; f[3] += _Value[3]; }
void v4::operator-=(const v4& _Value) { f[0] -= _Value[0]; f[1] -= _Value[1]; f[2] -= _Value[2]; f[3] -= _Value[3]; }
void v4::operator*=(const v4& _Value) { f[0] *= _Value[0]; f[1] *= _Value[1]; f[2] *= _Value[2]; f[3] *= _Value[3]; }
void v4::operator/=(const v4& _Value) { f[0] /= _Value[0]; f[1] /= _Value[1]; f[2] /= _Value[2]; f[3] /= _Value[3]; }

v4 v4::operator+(const v4& _Value) const { return v4( f[0]+_Value[0], f[1]+_Value[1], f[2]+_Value[2], f[3]+_Value[3] ); }
v4 v4::operator-(const v4& _Value) const { return v4( f[0]-_Value[0], f[1]-_Value[1], f[2]-_Value[2], f[3]-_Value[3] ); }
v4 v4::operator*(const v4& _Value) const { return v4( f[0]*_Value[0], f[1]*_Value[1], f[2]*_Value[2], f[3]*_Value[3] ); }
v4 v4::operator/(const v4& _Value) const { return v4( f[0]/_Value[0], f[1]/_Value[1], f[2]/_Value[2], f[3]/_Value[3] ); }

float v4::len2() const { return f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3]; }
float v4::len() const { float l2=len2(); return (l2>V4_EPSILON)?sqrtf(l2):0.f; }

v4 v4::unit() const { float l=len(); return (l>V4_EPSILON)?*this/l:v4(0.f); }
void v4::normalise() { float l=len(); if(l>V4_EPSILON) *this/=l; else *this=v4(0.f); }
v4 v4::canonicalise()
{
#define UNITY (1.0 - 1.0e-6)
	if     ( f[0] >=  UNITY ) return v4( 1,0,0,0);
	else if( f[0] <= -UNITY ) return v4(-1,0,0,0);
	else if( f[1] >=  UNITY ) return v4(0, 1,0,0);
	else if( f[1] <= -UNITY ) return v4(0,-1,0,0);
	else if( f[2] >=  UNITY ) return v4(0,0, 1,0);
	else if( f[2] <= -UNITY ) return v4(0,0,-1,0);
	else if( f[3] >=  UNITY ) return v4(0,0,0, 1);
	else if( f[3] <= -UNITY ) return v4(0,0,0,-1);
	else return v4::zero;
#undef UNITY
}
v4 v4::recip() const { return v4(1.f/f[0],1.f/f[1],1.f/f[2],1.f/f[3]); }
void v4::clamp(const v4& _Min,const v4& _Max)
{
	if(f[0]<_Min[0]) f[0]=_Min[0];
	if(f[0]>_Max[0]) f[0]=_Max[0];
	if(f[1]<_Min[1]) f[1]=_Min[1];
	if(f[1]>_Max[1]) f[1]=_Max[1];
	if(f[2]<_Min[2]) f[2]=_Min[2];
	if(f[2]>_Max[2]) f[2]=_Max[2];
	if(f[3]<_Min[3]) f[3]=_Min[3];
	if(f[3]>_Max[3]) f[3]=_Max[3];
}

void v4::print(string _String) const { printf( "%s %.3f %.3f %.3f %.3f\n", _String.c_str(), f[0], f[1], f[2], f[3] ); }
void v4::print(string _String)       { printf( "%s %.3f %.3f %.3f %.3f\n", _String.c_str(), f[0], f[1], f[2], f[3] ); }

/*** swizzling ***/
v2 v4::xy() const { return v2( f[0], f[1] ); }
v2 v4::yz() const { return v2( f[1], f[2] ); }
v2 v4::xz() const { return v2( f[0], f[2] ); }
v3 v4::xyz() const { return v3( f[0], f[1], f[2] ); }

#undef V4_EPSILON

} // namespace dalelib
